#ifndef TOPOLOGICAL_MAP_H
#define TOPOLOGICAL_MAP_H

#include <libplayerc++/playerc++.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <math.h>
#include<limits.h>
#include "path_planning/pixel.h"

using namespace std;
using namespace PlayerCc;

const double kUnknowValue = LONG_MIN;

struct AdjNode{
	double dist;
	int adjacent_id;
	AdjNode *next;
};


class AdjNodeList{
public:
	AdjNodeList() : first_(NULL), last_(NULL){}

	~AdjNodeList(void);

	void Add(int adjacent_id, double dist);

	AdjNode* Remove(int adjacent_id);

	AdjNode *GetFirst();

	AdjNode *GetLast();

private:
	AdjNode *first_;
	AdjNode *last_;

};

struct Vertex
{
	int id;
	double f,g,h;
	double x_coordinate, y_coordinate;
	Vertex *parent;
	AdjNodeList *list_of_adjacents;
};

class AdjacencyList{

public:
	AdjacencyList(int size);

	AdjacencyList(void);

	~AdjacencyList();

	bool AddEdge(int verta, int vertb);

	bool RemoveEdge(int verta, int vertb);

	Vertex *GetVertexById(int vertex_id);

	int edges() const;

	int size() const;

private:

	int size_;
	int edges_;
	int max_verts_;

	Vertex **list_of_vertex_;
};

class PlannerMap{

   public:

	   static const int kInvalidEdge = -1;

	   static const int kEdgeOk = -2;

	   PlannerMap() : topological_map_(NULL), grid_map_matrix_(NULL){ }

	   ~PlannerMap(void);

	   bool SetGraphDescriptor(string graph_descriptor_path_file);

	   bool SetGridMapDescriptor(string grid_map_path_file);

	   bool RemoveEdge(int vertex_1_id, int vertex_2_id);

	   bool AddEdge(int vertex_1_id, int vertex_2_id);

	   Vertex* GetClosestVertex(double x_coordinate, double y_coordinate);

	   void GetClosestVertices(player_point_2d origin, player_point_2d destination,
			   	   	   	   	   Vertex* &closest_vertex_to_origin, Vertex* &closest_vertex_to_destination);

	   AdjNodeList *GetAdjacents(int vertex_id);

	   Vertex *GetVertex(int vertex_id);

	   int GetEdgeCost(int vertex_1_id, int vertex_2_id, int *edge_cost);

	   int Size();

	   void Reset();

   private:

   	   AdjacencyList* topological_map_;

   	   double grid_map_resolution_;
   	   int grid_map_width_;
   	   int grid_map_height_;
   	   int* grid_map_matrix_;

   	   string graph_descriptor_path_file_;

   	   int number_of_vertices_;

};



#endif 
